#ifndef ML_CONJUGATE_GRADIENT_H
#define ML_CONJUGATE_GRADIENT_H

#include"vector.h"
#include"operations.h"
namespace math{

/*----------------------------------------------

  Solve linear equation Ax = b with linear conjugate gradient method, where
  A is an definite symmetric matrix.

*/
	template<class Mat, class Vec1, class Vec2>
	vector<typename result_value_type<double,
							 typename Mat::value_type,
							 typename Vec1::value_type,
							 typename Vec2::value_type>::type>
	cg_solver(const Mat& A, const Vec1& b, const Vec2& x0,
			  double tol = 1e-7)
	{
		typedef typename result_value_type<double,
							 typename Mat::value_type,
							 typename Vec1::value_type,
							 typename Vec2::value_type>::type value_type;
		typedef vector<value_type> result_type;
		result_type x(x0);
		result_type r = prod(A, x) - b;    //gradient
		result_type p = -r;		//search direction
		value_type r_norm = inner_prod(r, r); 
		value_type r_norm_new, p_norm_A, alpha, beta; //alpha is step length.
		result_type  Ap;
		while(r_norm > tol)
		{
			//calculate step-length and update x
			Ap = prod(A, p);
			p_norm_A = inner_prod(p, Ap);
			alpha = r_norm / p_norm_A;
			axpy(alpha, p, x);  //update x 

			//calculate next conjugate direction
			axpy(alpha, Ap, r);  //update r
			r_norm_new = inner_prod(r, r);
			beta = r_norm_new / r_norm;
			r_norm = r_norm_new;
			p *= beta;
			p -= r;
		}
		return x;
	}



		



}

#endif

